// ORBFeatureTracker.cpp
#include "ORBFeatureTracker.h"
#include <algorithm> 
ORBFeatureTracker::ORBFeatureTracker() {
    orb = cv::ORB::create(500); // 最大特征点数
    matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
}

void ORBFeatureTracker::detectAndTrack(cv::Mat& frame) {
    cv::Mat gray;
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

    // 检测ORB特征点
    std::vector<cv::KeyPoint> keypoints;
    cv::Mat descriptors;
    orb->detectAndCompute(gray, cv::noArray(), keypoints, descriptors);

    // 首帧初始化
    if (prevKeypoints.empty()) {
        prevKeypoints = keypoints;
        prevDescriptors = descriptors.clone();
        return;
    }

    // 特征匹配
    std::vector<cv::DMatch> matches;
    matcher->match(prevDescriptors, descriptors, matches);

    // 筛选优质匹配（距离 < 2倍最小距离）
    double min_dist = DBL_MAX;
    for (auto& m : matches) min_dist = (min_dist < m.distance)? min_dist : m.distance;
    std::vector<cv::DMatch> good_matches;
    for (auto& m : matches) {
        if (m.distance < 2 * min_dist) good_matches.push_back(m);
    }

    // 绘制特征点与匹配线
    cv::Mat output;
    cv::drawMatches(gray, prevKeypoints, gray, keypoints, good_matches, output);
    cv::imshow("Feature Tracking", output);

    // 更新前一帧数据
    prevKeypoints = keypoints;
    prevDescriptors = descriptors.clone();
}